# pragma once

# include "cpp_tools_Headers.h"

namespace cpp_tools
{
    class gridInterp
    {
    private:
        Eigen::VectorXd grid;
        Eigen::VectorXd val;
        double cycleStart;  // 外插循环区间起点
        double cycleEnd;    // 外插循环区间终点
        bool customCycleOK; // 自定义外插区间是否可用

        std::vector<int> idx;
        std::vector<int> step;

        /// @brief 检查数据错误
        void check()
        {
            for (size_t i = 1; i < grid.size(); i++)
            {
                if (grid(i - 1) >= grid(i))
                {
                    throw std::runtime_error("gridInterp 错误: 网格并不是单调增的\n");
                }
            }

            if (grid.array().isNaN().any() || val.array().isNaN().any())
            {
                throw std::runtime_error("gridInterp 错误: 输入数据有nan\n");
            }
        }

    public:
        gridInterp()
            : customCycleOK(false), cycleStart(0), cycleEnd(0)
        {
            grid.resize(1);
            val.resize(1);
        };

        gridInterp(const Eigen::VectorXd &xq, const Eigen::VectorXd &vq)
            : grid(xq), val(vq), customCycleOK(false), cycleStart(0), cycleEnd(0)
        {
            if (xq.size() != vq.size())
            {
                throw std::runtime_error("gridInterp 错误: size of xq is not consistent with vq\n");
            }

            check();
        }

        ~gridInterp(){};

        /// @brief 重新设置插值数据
        /// @param xq 网格点，必须是按照递增顺序
        /// @param vq 网格点对应的值
        void reset(const Eigen::VectorXd &xq, const Eigen::VectorXd &vq)
        {
            if (xq.size() != vq.size())
            {
                throw std::runtime_error("gridInterp 错误: size of xq is not consistent with vq\n");
            }

            grid = xq;
            val = vq;
            
            check();
        }

        /// @brief 进行插值（默认外插使用边界值）
        /// @param xi 插值点
        /// @return 插值结果
        Eigen::VectorXd interp(const Eigen::VectorXd &xi)
        {
            // 原有实现保持不变
            int n = xi.size();
            idx.resize(n);
            step.resize(n);

            Eigen::VectorXd vi(n);

            for (size_t i = 0; i < n; i++)
            {
                int heading = 0;
                if (step[i] == 0)
                {
                    step[i] = std::max(std::floor(n/4), 1.);
                }

                while (1)
                {
                    if (xi(i) < grid(0))
                    {
                        idx[i] = 0;
                        vi(i) = val(0);
                        break;
                    }
                    else if (xi(i) >= grid(Eigen::last))
                    {
                        idx[i] = val.size() - 1;
                        vi(i) = val(Eigen::last);
                        break;
                    }
                    else if (xi(i) >= grid(idx[i]))
                    {
                        if (xi(i) < grid(idx[i] + 1))
                        {
                            vi(i) = (xi(i) - grid(idx[i]))*(val(idx[i] + 1) - val(idx[i]))/(grid(idx[i] + 1) - grid(idx[i])) + val(idx[i]);
                            break;
                        }
                        else
                        {
                            if (heading < 0)
                            {
                                step[i] = std::max(std::floor(step[i]/2), 1.);
                            }
                            heading = 1;
                            idx[i] = std::min(idx[i] + step[i], (int)val.size() - 1);
                        }
                    }
                    else if (xi(i) < grid(idx[i]))
                    {
                        if (heading > 0)
                        {
                            step[i] = std::max(std::floor(step[i]/2), 1.);
                        }
                        heading = -1;
                        idx[i] = std::max(idx[i] - step[i], 0);
                    }
                }
            }

            return vi;
        }

        /// @brief 单个数据插值（默认外插使用边界值）
        double interp(const double xi)
        {
            Eigen::VectorXd tmp(1);
            tmp << xi;
            return interp(tmp)(0);
        }
    };
} // namespace cpp_tools
